/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* Plot signalmap based on ESSID
* - Reads two logfiles recorded with MaCI_Logger 1) Position and 2) Wireless logfiles
* - Uses WirelessPositionLog Class to read the files 
* - Uses given ESSID string to put the signal values into a Wireless heatmap
* - plots the result 
**/
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include "owndebug.h"
#include "WirelessPositionLog.h"
#include "TOccupancyGrid.h"
#include "glutImageGUI.h"   ///< For visualisation
#include "JPEGEncoder.hpp"  ///< For saving JPEG

///Occu
TOccupancyGrid *heatmap;
double grid_resolution = 0.5;
double grid_size = 50; ///< in case the line map is unspecified
///Heatmap
float RSS_range=1.5;

float areaAverage(TOccupancyGrid *g, int pix, int piy, float r, float resolution){
	int n=(int)(r/resolution);
	int cnt=0;
	float avg=0;
	float *dptr = g->getDataPtr();
	int Nx,Ny;
	g->getDim(Nx,Ny);
	
	float v;
	
	for(int y=piy-n;y<piy+n;y++){
		for(int x = pix-n;x<pix+n;x++){
			if(g->getValue(Grid::TPixel(x,y),v)){
				if(v!=255){
					avg+=v; cnt++;
				}
			}
			 
		}
	}

	return (avg/cnt);
}



/**
 * find index of maximum RSS value 
**/
int findMaxIndex(TOccupancyGrid *g){
	float m=0;
	int mi=0;
	float *dptr=g->getDataPtr();
	int Nx,Ny;
	g->getDim(Nx,Ny);
	for(int i=0;i<g->size();i++){
		float avg=areaAverage(g, i%Nx, i/Nx, 2.5, grid_resolution);
		if(avg>m){
			m=avg;
			mi=i;
		}
	
	}
	
	return mi;
}


void printUsage(){
	fprintf(stderr,"\tPlot signalmap based on ESSID\n");
	fprintf(stderr,"\t\tReads two logfiles recorded with MaCI_Logger 1) Position and 2) Wireless logfiles \n");
	fprintf(stderr,"\t\tUses WirelessPositionLog Class to read the files \n");
	fprintf(stderr,"\t\tUses given ESSID string to put the signal values into a Wireless heatmap \n");
	fprintf(stderr,"\t\tplots the result\n");
	fprintf(stderr,"\n");
	fprintf(stderr,"USAGE: ./plotWirelessSignalMap -p [name] -w [name] [opts]\n");
	fprintf(stderr,"\t\t\t[-p name] name of position logfile\n");
	fprintf(stderr,"\t\t\t[-w name] name of the wireless logfile\n");
	fprintf(stderr,"\t\t\t[-e essid]  ESSID that you want to print\n");
	fprintf(stderr,"\t\t\t[-m map]  Line map name\n");
	fprintf(stderr,"\t\t\t[-l] list of Keys (MAC address) with essid\n");
	fprintf(stderr,"\t\t\t[-h/-?]  usage\n\n");
	//fprintf(stderr,"\t\t\t[-l] \n");
}

void saveJPEG(const char *fname, unsigned char *rgb,int Nx, int Ny){
	CJPEGEncoder encoder;
	encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
	FILE *f = fopen(fname,"wb");
	fwrite(encoder.GetJPEGDataPtr(), 
				 encoder.GetJPEGDataSize(), 
				 1, f);
	fclose(f);
		
}

/**
* Displays occumap forever using gray scale
**/
void displayOccuMap(TOccupancyGrid *map){
	int Nx,Ny;	
	unsigned char *rgb;
	map->getDim(Nx,Ny);
	rgb = (unsigned char *) malloc(3*Nx*Ny);
	
	map->fillRGB(rgb,1.0);
	
	int ind = findMaxIndex(map);
	rgb[3*ind] = 255;
	rgb[3*ind+1] = 0;
	rgb[3*ind+2] = 0;
	saveJPEG("out_result.jpg", rgb,Nx,Ny);
	
	addImageWindow(Nx, Ny);
	glutImageGUI_Start();
		
	fprintf(stderr,"\n ######   CTRL-C Ends the show   ######\n");
	
	while(1){
		displayImage(0, rgb,Ny,Nx);
		usleep(1000*500);
	}
}
/////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////
/// Heatmap generation funftions
/////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////
float *weight;

void updatePixelValue(TOccupancyGrid *g, int xx, int yy, float RSS, float w){
	grid_t value;
	int Nx,Ny;
	g->getDim(Nx,Ny);
	unsigned int index = Nx*yy+xx; 
			
	if(g->getValue(Grid::TPixel(xx,yy),value) ){
		if(value==0){
			g->setValue(Grid::TPixel(xx,yy),RSS);
			weight[index] = w;
		}
		else if(value != 255){
			if(w+weight[index] == 0) dPrint(1,"Warning Division with zero");
			g->setValue(Grid::TPixel(xx,yy),(w*RSS+weight[index]*value)/(w+weight[index]));
			if(w>weight[index]) weight[index]=w;
		}
	}
	
}

/**
 * Draws a circle to grid.
 * @param pix start pixel 
 * @param piy start pixel
 * @param r metric radius of the circle
 * @param resolution how many meters is one pixel
 */
void setFilledCircle(TOccupancyGrid *g, int pix, int piy, float r, float resolution,float RSS){
	int radius = (int) (r/resolution);
	while(radius>1){
		int i, j; 
		float diff2, diff;			
		for (i = pix - radius; i <= pix + radius; ++i) {
			for(float y=i; y<i+1;y+=0.24){
				diff2 = (radius * radius) - ((y - pix) * (y - pix));
				diff = (int) sqrt(((double) diff2));
				j = piy - diff;
				float r2 = radius*resolution*radius*resolution;
				float w = exp(-r2/(0.6*0.6));
				updatePixelValue(g,i,j,RSS,w);
								
				j = piy + diff;
				updatePixelValue(g,i,j,RSS,w);
			}
		}
		radius--;
	}
}

void setBlock(TOccupancyGrid *g, int pix, int piy, float r, float resolution,float RSS){
	int n=(int)(r/resolution);
	for(int y=piy-n;y<piy+n;y++){
		for(int x = pix-n;x<pix+n;x++){
			float r2 = ((x-pix)*(x-pix)+(y-piy)*(y-piy))*resolution*resolution;
			float w = exp(-r2/(0.6*0.6));
			updatePixelValue(g,x,y,RSS,w);
		}
	}
}


void setRSSMeasurement(TOccupancyGrid *g, float x, float y, uint32_t rss){
	Grid::TPixel p=g->toPixel(Grid::TCoord2D(x,y));
	//setFilledCircle(g,p.x,p.y,RSS_range,grid_resolution,rss);
	setBlock(g,p.x,p.y,RSS_range,grid_resolution,rss);
}

/////////////////////////////////////////////////////////////////////////////////////////

/**************************************************************************************
* MAIN
****************************************************************************************/
int main(int argc, char *argv[]){
	int verbose = 1;
	std::string positionLogfile;
	std::string wirelessLogfile;
	std::string linemapname;
	std::string essid_name;
	bool printIDs=false;
	bool hasLineMap=false;
	
	// Init Debug lib
	debugInit();
	debugSetGlobalDebugLvl(verbose);
			
	// Parse parameters
	bool parse = true;
	while(parse) {
		int c;
		c = getopt(argc, argv, "p:w:e:m:hlvq");
		switch(c) {
		case 'p': {
				positionLogfile = optarg;
				fprintf(stderr,"Using %s as Position logfile\n",
																			positionLogfile.c_str());
				break;
			}
			case 'w': {
				wirelessLogfile = optarg;
				fprintf(stderr,"Using %s as wireless logfile\n",
																	wirelessLogfile.c_str());
				break;
			}
			case 'e': {
				essid_name = optarg;
				fprintf(stderr,"Using %s as ESSID Name\n",essid_name.c_str());
				break;
			} 
			case 'm': {
				linemapname = optarg;
				hasLineMap = true;
				fprintf(stderr,"Using %s as Line map Name\n",linemapname.c_str());
				break;
			}   
			case 'l':
				printIDs = true;
				break;

			case 'v':
      		debugSetGlobalDebugLvl(++verbose);
				break;
      
			case 'q':
				verbose = 0;
				debugSetGlobalDebugLvl(0);
				break;
      
			case 'h':
			case '?':
				printUsage();
				exit(1);
				break;
      
			case -1:
				parse = false;
				break;
		}
	}
	
	dPrint(1,"Logfiles: Position = %s Wireless = %s",
				 positionLogfile.c_str(),wirelessLogfile.c_str());
	dPrint(1,"ESSID = %s",essid_name.c_str());
	
	CWirelessPositionLog log(wirelessLogfile, positionLogfile);
	
	if(printIDs){
		for(unsigned int i=0; i<log.signalKeys.size();i++){
			fprintf(stderr,"MAC=%llx ESSID=%s NumMeas=%d \n",log.signalKeys[i],log.getESSID(log.signalKeys[i]).c_str(),log.getNumMeasurements(log.signalKeys[i]));
		}
		return 0;
	}
	heatmap = new TOccupancyGrid(grid_size, grid_size, grid_resolution, Grid::TCoord2D(0,0));
	int Nx,Ny;
	heatmap->getDim(Nx,Ny);
	weight = (float*)malloc(Nx*Ny*sizeof(float));	
	memset ( (void *)weight, 0, Nx*Ny*sizeof(float) );
	fprintf(stderr,"grid_size = %dx%d\n",Nx,Ny);
	
	if(hasLineMap){
		heatmap->loadLineMap(linemapname.c_str(), 1.0,255,true,true,0);
	}
	
	///Go through all keys
	for(unsigned int i=0; i<log.signalKeys.size();i++){
		///If the key matches with the essid
		//dPrint(1,"Searching '%s' I have '%s'",log.getESSID(log.signalKeys[i]).c_str(),
		//																											essid_name.c_str());
		
		if(log.getESSID(log.signalKeys[i]) == essid_name){
			std::pair<std::multimap<uint64_t, SignalStrengthWithPosition>::iterator, 
			std::multimap<uint64_t, SignalStrengthWithPosition>::iterator> partial;
			
			partial = log.signalMap.equal_range(log.signalKeys[i]); ///Get all values of this key

			for (std::multimap<uint64_t, SignalStrengthWithPosition>::iterator it2 = partial.first;
							it2 != partial.second; ++it2)
			{
				dPrint(10,"%llx=%s %f %f %d",
							 (*it2).first,(*it2).second.ESSID.c_str(), 
							 (*it2).second.x,(*it2).second.y, (*it2).second.rss);
				setRSSMeasurement(heatmap,(*it2).second.x,(*it2).second.y,((*it2).second.rss-150)*2.5);
						
			}
		}
	}
	
	
	
	displayOccuMap(heatmap);
	
	return 0;
}
